#include "ros/ros.h"
#include "custom_msgs/AddTwoInts.h"
#include <cstdlib> // 用于 atoi()

int main(int argc, char **argv)
{
    // 初始化 ROS 节点
    ros::init(argc, argv, "add_two_ints_client");
    ros::NodeHandle nh;

    // 检查命令行参数
    if (argc != 3)
    {
        ROS_ERROR("Usage: add_two_ints_client X Y");
        return 1;
    }

    // 检查 ROS Master 是否可用
    if (!ros::master::check())
    {
        ROS_FATAL("Failed to connect to ROS Master! Is roscore running?");
        return 1;
    }

    // 创建服务客户端
    ros::ServiceClient client = nh.serviceClient<custom_msgs::AddTwoInts>("add_two_ints");

    // 构造请求数据
    custom_msgs::AddTwoInts srv;
    srv.request.a = atoll(argv[1]);
    srv.request.b = atoll(argv[2]);

    // 等待服务可用（带超时机制）
    const double MAX_WAIT_TIME = 10.0; // 最大等待 10 秒
    double wait_time = 0.0;
    ros::Rate loop_rate(1.0); // 1 Hz

    while (!client.waitForExistence(ros::Duration(1.0)))
    {
        // ROS_WARN("Waiting for service 'add_two_ints' to become available...");
        wait_time += 1.0;
        if (wait_time >= MAX_WAIT_TIME)
        {
            ROS_FATAL("Service 'add_two_ints' not available after %.1f seconds!", wait_time);
            return 1;
        }
        loop_rate.sleep();
    }

    // 调用服务
    if (client.call(srv))
    {
        ROS_INFO("Success: %ld + %ld = %ld", srv.request.a, srv.request.b, srv.response.sum);
    }
    else
    {
        ROS_ERROR("Failed to call service 'add_two_ints'");
        return 1;
    }

    return 0;
}